﻿using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.Optimization;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;

namespace ThreadModule.Data
{
    public abstract class ThreadBase
    {
        /// <summary>
        /// 齿高
        /// </summary>
        protected double _toothHeight;
        /// <summary>
        /// Y判定阈值
        /// </summary>
        protected double _threshodY;
        /// <summary>
        /// 角度判断阈值
        /// </summary>
        protected double _threshodAngle;
        /// <summary>
        /// 全角角度值
        /// </summary>
        protected double _fullAngle { get; set; }
        /// <summary>
        /// 采样密度值
        /// </summary>
        protected double _samplingDensity { get; set; }

        /// <summary>
        /// 大径拟合数据点数，默认为5
        /// </summary>
        protected int _rangeMajor;

        /// <summary>
        /// 螺纹参数
        /// </summary>
        protected ThreadParameters _threadNominalParameters;

        /// <summary>
        /// 全角参数值
        /// </summary>
        public List<Tuple<double, double>> listFullAngleParameters { get; set; }

        /// <summary>
        /// 全角参数集合
        /// </summary>
        protected List<Tuple<double, double>> _listFullAngleParameters;

        /// <summary>
        /// 计算模式
        /// </summary>
        protected CalculationMethod _calculationMethod;

        /// <summary>
        /// 量规类型
        /// </summary>
        private ThreadGaugeType _threadGaugeType;

        /// <summary>
        /// 极值点位置
        /// </summary>
        public List<int> listExtremPointsIndex { get; set; }

        /// <summary>
        /// 拟合牙侧角数据点
        /// </summary>
        protected List<List<RawDataPoint>> _listFitAnglePoints;

        /// <summary>
        /// ThreadBase构造参数，包含三个参数
        /// </summary>
        /// <param name="threadInformation">螺纹信息</param>
        /// <param name="threshodAngle">角度判断阈值，默认为2.0度</param>
        /// <param name="calculationMethod">中径计算方式，包括迭代、三针，默认为迭代</param>
        public ThreadBase(ref ThreadInformation threadInformation, double threshodAngle = 2.0, CalculationMethod calculationMethod = CalculationMethod.Iterator)
        {
            this._threadGaugeType = threadInformation.guageType;
            this._threshodAngle = threshodAngle;
            this._calculationMethod = calculationMethod;
            this._threadNominalParameters = threadInformation.nominalThreadParameter;

            this._samplingDensity = (GetSamplingDensity(threadInformation.topThreadRawData.listRawData) + GetSamplingDensity(threadInformation.bottomThreadRawData.listRawData)) / 2;
            this._fullAngle = threadInformation.nominalThreadParameter.fullAngle;
            this._threshodY = this.GetThreshod(this._fullAngle);
            this._rangeMajor = 5;
        }

        /// <summary>
        /// 执行计算
        /// </summary>
        /// <param name="threadInformation">螺纹信息</param>
        public virtual void Run(ref ThreadInformation threadInformation)
        {
            ThreadParameters measuredThreadParameter = new ThreadParameters();

            CalculateThreadParameters(ref threadInformation, Position.Top);
            CalculateThreadParameters(ref threadInformation, Position.Bottom);

            measuredThreadParameter.isNominalValues = false;//为实测值
            var calculatedTopParameter = threadInformation.topCalculatedParameter;
            var calculatedBottomParameter = threadInformation.bottomCalculatedParameter;
            ShowCalculatedParameters(calculatedTopParameter);
            ShowCalculatedParameters(calculatedBottomParameter);

            measuredThreadParameter.pitchDiameter = ThreadBase.GetPitchDiameterValue(ref threadInformation);
            measuredThreadParameter.majorDiameter = ThreadBase.GetMajorDiameterValue(ref threadInformation);
            measuredThreadParameter.minorDiameter = ThreadBase.GetMinorDiameterValue(ref threadInformation);
            measuredThreadParameter.pitch = (calculatedBottomParameter.pitch + calculatedTopParameter.pitch) / 2;
            measuredThreadParameter.leftHalfAngle = (calculatedBottomParameter.leftHalfAngle + calculatedTopParameter.leftHalfAngle) / 2;
            measuredThreadParameter.rightHalfAngle = (calculatedBottomParameter.rightHalfAngle + calculatedTopParameter.rightHalfAngle) / 2;
            measuredThreadParameter.fullAngle = calculatedBottomParameter.fullAngle + calculatedTopParameter.fullAngle;
            threadInformation.measuredThreadParameter = measuredThreadParameter;
            ShowOutputParameters(measuredThreadParameter);
        }

        /// <summary>
        /// 获取中径值
        /// </summary>
        /// <param name="threadInformation">螺纹信息</param>
        /// <returns>中径值</returns>
        protected static double GetPitchDiameterValue(ref ThreadInformation threadInformation)
        {
            var calculatedTopParameter = threadInformation.topCalculatedParameter;
            var calculatedBottomParameter = threadInformation.bottomCalculatedParameter;
            double b1 = calculatedTopParameter.paramPitchDiameter.Item2 * GetCenterOfGravity(threadInformation.topThreadRawData.listRawData) + calculatedTopParameter.paramPitchDiameter.Item1;
            double b2 = calculatedBottomParameter.paramPitchDiameter.Item2 * GetCenterOfGravity(threadInformation.bottomThreadRawData.listRawData) + calculatedBottomParameter.paramPitchDiameter.Item1;
            return Math.Abs(b1 - b2);
        }

        /// <summary>
        /// 获取大径值
        /// </summary>
        /// <param name="threadInformation">螺纹信息</param>
        /// <returns>大径值</returns>
        protected static double GetMajorDiameterValue(ref ThreadInformation threadInformation)
        {
            var calculatedTopParameter = threadInformation.topCalculatedParameter;
            var calculatedBottomParameter = threadInformation.bottomCalculatedParameter;
            double b1 = calculatedTopParameter.paramMajor.Item2 * GetCenterOfGravity(threadInformation.topThreadRawData.listRawData) + calculatedTopParameter.paramMajor.Item1;
            double b2 = calculatedBottomParameter.paramMajor.Item2 * GetCenterOfGravity(threadInformation.bottomThreadRawData.listRawData) + calculatedBottomParameter.paramMajor.Item1;
            return Math.Abs(b1 - b2);
        }

        /// <summary>
        /// 获取小径值
        /// </summary>
        /// <param name="threadInformation">螺纹信息</param>
        /// <returns>小径值</returns>
        protected static double GetMinorDiameterValue(ref ThreadInformation threadInformation)
        {
            var calculatedTopParameter = threadInformation.topCalculatedParameter;
            var calculatedBottomParameter = threadInformation.bottomCalculatedParameter;
            double b1 = calculatedTopParameter.paramMinor.Item2 * GetCenterOfGravity(threadInformation.topThreadRawData.listRawData) + calculatedTopParameter.paramMinor.Item1;
            double b2 = calculatedBottomParameter.paramMinor.Item2 * GetCenterOfGravity(threadInformation.bottomThreadRawData.listRawData) + calculatedBottomParameter.paramMinor.Item1;
            return Math.Abs(b1 - b2);
        }

        /// <summary>
        /// 根据原始数据点计算采样率
        /// </summary>
        /// <param name="listRawdataPoints">原始数据点集</param>
        /// <returns>采样点密度</returns>
        private double GetSamplingDensity(List<RawDataPoint> listRawdataPoints)
        {
            int range = 20;
            double samplingDensity = 0.0;
            if (listRawdataPoints.Count > range)
            {
                // 使用LINQ计算前十个数的后项减前项的差值
                samplingDensity = listRawdataPoints
                    .Select((value, index) => new { Index = index, Value = value })
                    .Where(pair => pair.Index < 20)
                    .Select(pair => Math.Abs(pair.Value.X - listRawdataPoints[pair.Index + 1].X)).Average(); // 计算差值
            }
            return samplingDensity;
        }

        /// <summary>
        /// 寻找第一个极值点的位置
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="pos">截取位置</param>
        /// <param name="range">取点数量，默认值为20个点</param>
        /// <returns>第一个极值点的index序号</returns>
        protected int FindFirstPointIndex(List<RawDataPoint> listRawdata, double pos, int range = 20)
        {
            int posCheck = 1;//斜率方向判断
            List<int> listExtremPointsIndex = new List<int>();
            #region 寻找相交点算法
            double nCounter = 0;
            bool checkThreadGaugeType = this._threadGaugeType.ToString().Contains("Plug") ? true : false;//判定量规类型
            //初始方向判定，外螺纹为-1，内螺纹为+1
            if (checkThreadGaugeType)
            {
                posCheck = -1;
            }
            else
            {
                posCheck = 1;
            }

            //判断点是否在均值附近，如果在取附近十个计算斜率。每个牙侧角附近只取符合条件的一个点。
            for (int i = 0; i < listRawdata.Count; i++)
            {
                double y = listRawdata[i].Y;
                double dist = Math.Abs(y - pos);
                //距离在阈值范围内
                if (dist <= _threshodY)
                {
                    var values = listRawdata.Skip(i).Take(range).ToList();
                    var xList = values.Select(n => n.X).ToArray();
                    var yList = values.Select(n => n.Y).ToArray();
                    var param = Fit.Line(xList, yList);

                    //方向判断
                    if (param.Item2 * posCheck > 0)
                    {
                        double angle = AngleConverter.ConvertRadToDegree(param.Item2);
                        //角度判断
                        if (angle < 0)
                        {
                            angle = 180 - angle;
                        }
                        //判定是否在阈值范围内
                        if (angle > _threadNominalParameters.fullAngle - _threshodAngle || angle < _threadNominalParameters.fullAngle + _threshodAngle)
                        {
                            //取三个值，第一第三个值的差值为一个螺距所对应的index。
                            if (nCounter == 2)
                            {
                                break;
                            }
                            else
                            {
                                listExtremPointsIndex.Add(i);
                                posCheck = -posCheck;//反转方向，保证每个牙侧面只取一个值
                                nCounter++;
                            }
                        }
                    }
                }
            }
            #endregion
            return (int)listExtremPointsIndex.Average();
        }

        /// <summary>
        /// 获取极值点位置
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="firstIndex">第一个极值位置index</param>
        /// <param name="pointInterval">点集跨度区间，即采样点密度x螺距</param>
        /// <returns>选取范围</returns>
        public List<int> FindExtremPointsIndex(List<RawDataPoint> listRawdata, int firstIndex, int pointInterval)
        {
            List<int> listIndex = new List<int>();
            for (int i = firstIndex; i < listRawdata.Count; i += pointInterval)
            {
                listIndex.Add(i);
            }
            return listIndex;
        }

        /// <summary>
        /// 计算螺纹参数
        /// </summary>
        /// <param name="threadInformation">螺纹相关信息</param>
        /// <param name="position">测量位置</param>
        protected virtual void CalculateThreadParameters(ref ThreadInformation threadInformation, Position position)
        {
            CalculatedThreadParameter calculatedParameter = new CalculatedThreadParameter();
            calculatedParameter.position = position;

            #region 螺纹计算
            var listRawdata = new List<RawDataPoint>();
            switch (position)
            {
                case Position.Top:
                    listRawdata = threadInformation.topThreadRawData.listRawData;
                    break;
                case Position.Bottom:
                    listRawdata = threadInformation.bottomThreadRawData.listRawData;
                    break;
            }

            _toothHeight = CalculateToothHeight(threadInformation);

            int firstIndex = FindFirstPointIndex(listRawdata, GetAverage(listRawdata));
            int interval = (int)(threadInformation.nominalThreadParameter.pitch / _samplingDensity / 2);//按照半个螺距获取
            listExtremPointsIndex = FindExtremPointsIndex(listRawdata, firstIndex, interval);

            //计算大径小径
            List<RawDataPoint> listFitMajorPoints;
            List<RawDataPoint> listFitMinorPoints;
            var paramMajor = FitMajorOrMinorLine(listRawdata, listExtremPointsIndex, out listFitMajorPoints, position, SearchMode.Major);
            var paramMinor = FitMajorOrMinorLine(listRawdata, listExtremPointsIndex,out listFitMinorPoints, position, SearchMode.Minor);

            //计算中径
            var paramPitchDiameter = CalculatePitchDiameter(listRawdata, paramMajor, position);

            var listPitch = CalculatePitch(paramPitchDiameter, _listFullAngleParameters, position);
            var listHalfAngle = CalculateHalfAngle(paramPitchDiameter, _listFullAngleParameters, position);

            calculatedParameter.listFullAngleParameters = _listFullAngleParameters;
            calculatedParameter.listPitch = listPitch;
            calculatedParameter.pitch = calculatedParameter.GetPitch();
            calculatedParameter.paramMajor = paramMajor;
            calculatedParameter.paramMinor = paramMinor;
            calculatedParameter.paramPitchDiameter = paramPitchDiameter;
            calculatedParameter.listHalfAngle = listHalfAngle;
            calculatedParameter.leftHalfAngle = calculatedParameter.GetHalfAngle(true);
            calculatedParameter.rightHalfAngle = calculatedParameter.GetHalfAngle(false);
            calculatedParameter.fullAngle = calculatedParameter.GetFullAngle();
            calculatedParameter.listFitMajorPoints = listFitMajorPoints;
            calculatedParameter.listFitMinorPoints = listFitMinorPoints;
            calculatedParameter.listFitAnglePoints = _listFitAnglePoints;
            //ShowCalculatedParameters(calculatedParameter);

            switch (position)
            {
                case Position.Top:
                    threadInformation.topCalculatedParameter = calculatedParameter;
                    break;
                case Position.Bottom:
                    threadInformation.bottomCalculatedParameter = calculatedParameter;
                    break;
                default:
                    break;
            }
            #endregion
        }

        /// <summary>
        /// 计算半角
        /// </summary>
        /// <param name="paramPitchDiameter">中径参数</param>
        /// <param name="listFullAngleParameters">全角参数集合</param>
        /// <param name="position">位置(上/下)</param>
        /// <returns></returns>
        protected List<double> CalculateHalfAngle(Tuple<double, double> paramPitchDiameter, List<Tuple<double, double>> listFullAngleParameters, Position position)
        {
            bool checkCondition = listFullAngleParameters.First().Item2 > 0;
            int nStart;
            if (checkCondition)
            {
                if (position == Position.Top)
                {
                    nStart = 0;
                }
                else
                {
                    nStart = 1;
                }
            }
            else
            {
                if (position == Position.Top)
                {
                    nStart = 1;
                }
                else
                {
                    nStart = 0;
                }
            }

            List<double> listHalfAngle = new List<double>();
            for (int i = nStart; i < listFullAngleParameters.Count - 1; i += 2)
            {
                double k1 = listFullAngleParameters[i].Item2;
                double k2 = listFullAngleParameters[i + 1].Item2;
                double kPitch = paramPitchDiameter.Item2;

                double angleLeft = GetAngleBetweenLines(k1, -1 / kPitch);
                listHalfAngle.Add(angleLeft);
                double angleRight = GetAngleBetweenLines(k2, -1 / kPitch);
                listHalfAngle.Add(angleRight);
            }

            return listHalfAngle;
        }

        /// <summary>
        /// 计算螺距
        /// </summary>
        /// <param name="paramPitchDiameter">中径参数集合</param>
        /// <param name="listFullAngleParameters">半角斜率参数集合</param>
        /// <param name="position">位置(上/下)</param>
        /// <returns></returns>
        protected List<double> CalculatePitch(Tuple<double, double> paramPitchDiameter, List<Tuple<double, double>> listFullAngleParameters, Position position)
        {
            bool checkCondition = listFullAngleParameters.First().Item2 < 0;
            int nStart;
            if (checkCondition)
            {
                if (position == Position.Top)
                {
                    nStart = 0;
                }
                else
                {
                    nStart = 1;
                }
            }
            else
            {
                if (position == Position.Top)
                {
                    nStart = 1;
                }
                else
                {
                    nStart = 0;
                }
            }

            List<double> listPitch = new List<double>();
            for (int i = nStart; i < listFullAngleParameters.Count - 2; i += 2)
            {
                var paramLine1 = listFullAngleParameters[i];
                var paramLine2 = listFullAngleParameters[i + 2];
                double k1 = paramLine1.Item2;
                double k2 = paramLine2.Item2;
                double b1 = paramLine1.Item1;
                double b2 = paramLine2.Item1;
                double kdm = paramPitchDiameter.Item2;
                double bdm = paramPitchDiameter.Item1;

                RawDataPoint intersectPoint1 = GetIntersectPoint(k1, b1, kdm, bdm);
                RawDataPoint intersectPoint2 = GetIntersectPoint(k2, b2, kdm, bdm);
                double dist = Math.Sqrt(Math.Pow(intersectPoint1.X - intersectPoint2.X, 2) + Math.Pow(intersectPoint1.Y - intersectPoint2.Y, 2));
                listPitch.Add(dist);
            }

            return listPitch;
        }

        /// <summary>
        /// 拟合大径或小径线
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="listIndex">极值点位置</param>
        /// <param name="listMajorOrMinorFitPoints">大径或者小径拟合数据点</param>
        /// <param name="position">位置（上/下）</param>
        /// <param name="searchMode">搜索模式（大径/小径）</param>
        /// <returns>大径/小径值</returns>
        protected Tuple<double, double> FitMajorOrMinorLine(List<RawDataPoint> listRawdata, List<int> listIndex,out List<RawDataPoint> listMajorOrMinorFitPoints, Position position, SearchMode searchMode)
        {
            //_rangeMajor = 5;
            List<RawDataPoint> listFitdata1 = new List<RawDataPoint>();
            List<RawDataPoint> listFitdata2 = new List<RawDataPoint>();
            for (int i = 0; i < listIndex.Count - 1; i += 2)
            {
                int index1 = listIndex[i];
                int index2 = listIndex[i + 1];
                var values1 = listRawdata.Skip(index1 - _rangeMajor / 2).Take(_rangeMajor);
                var values2 = listRawdata.Skip(index2 - _rangeMajor / 2).Take(_rangeMajor);
                listFitdata1.AddRange(values1);
                listFitdata2.AddRange(values2);
            }
            double[] xList1 = listFitdata1.Select(n => n.X).ToArray();
            double[] yList1 = listFitdata1.Select(n => n.Y).ToArray();
            var param1 = Fit.Line(xList1, yList1);
            double[] xList2 = listFitdata2.Select(n => n.X).ToArray();
            double[] yList2 = listFitdata2.Select(n => n.Y).ToArray();
            var param2 = Fit.Line(xList2, yList2);

            Tuple<double, double> param;
            //根据情况判断

            if (position == Position.Top)
            {
                if (searchMode == SearchMode.Major)
                {
                    param = param1.Item1 > param2.Item1 ? param1 : param2;
                    listMajorOrMinorFitPoints = param1.Item1 > param2.Item1 ? listFitdata1 : listFitdata2;
                }
                else
                {
                    param = param1.Item1 < param2.Item1 ? param1 : param2;
                    listMajorOrMinorFitPoints = param1.Item1 < param2.Item1 ? listFitdata1 : listFitdata2;
                }
            }
            else
            {
                if (searchMode == SearchMode.Major)
                {
                    param = param1.Item1 < param2.Item1 ? param1 : param2;
                    listMajorOrMinorFitPoints = param1.Item1 < param2.Item1 ? listFitdata1 : listFitdata2;
                }
                else
                {
                    param = param1.Item1 > param2.Item1 ? param1 : param2;
                    listMajorOrMinorFitPoints = param1.Item1 > param2.Item1 ? listFitdata1 : listFitdata2;
                }
            }
            return param;
        }

        /// <summary>
        /// 计算齿高
        /// </summary>
        /// <param name="threadInformation">螺纹信息</param>
        protected double CalculateToothHeight(ThreadInformation threadInformation)
        {
            double h = 0.0;
            ThreadGaugeType type = threadInformation.guageType;
            double fullAngle = threadInformation.nominalThreadParameter.fullAngle;
            double pitch = threadInformation.nominalThreadParameter.pitch;
            if (type == ThreadGaugeType.CylinderPlug || type == ThreadGaugeType.CylinderRing)
            {
                if (fullAngle == 60.0)
                {
                    h = 5.0 / 8.0 * Math.Sqrt(3) / 2 * pitch;
                }
                else if (fullAngle == 55.0)
                {
                    h = 0.640327 * pitch;
                }
            }
            else if (type == ThreadGaugeType.TaperPlug || type == ThreadGaugeType.TaperRing)
            {
                if (fullAngle == 60.0)
                {
                    h = 5.0 / 8.0 * Math.Sqrt(3) / 2 * pitch;
                }
                else if (fullAngle == 55.0)
                {
                    h = 0.640327 * pitch;
                }
            }
            return h;
        }

        /// <summary>
        /// 获取deltaY，即相邻两点之间查找的阈值
        /// </summary>
        /// <param name="fullAngle">全角值（角度）</param>
        /// <param name="taper">锥度值</param>
        /// <returns>相邻数据点Y阈值</returns>
        protected virtual double GetThreshod(double fullAngle, double taper = 0)
        {
            return _samplingDensity / 2 * Math.Tan(AngleConverter.ConvertDegreeToRad(fullAngle));
        }

        /// <summary>
        /// 寻找符合条件的点
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="k">斜率值</param>
        /// <param name="b">截距值</param>
        /// <param name="range">取值范围</param>
        /// <returns>查找的点集，只查找符合条件的前三个</returns>
        private List<RawDataPoint> FindPoints(List<RawDataPoint> listRawdata, double k, double b, int range)
        {
            int posCheck = 0;//斜率方向判断
            List<RawDataPoint> listExtremPoints = new List<RawDataPoint>();

            #region 寻找相交点算法
            double nCounter = 0;
            //判断点是否在均值附近，如果在取附近十个计算斜率。每个牙侧角附近只取符合条件的一个点。
            for (int i = 0; i < listRawdata.Count; i++)
            {
                double y = listRawdata[i].Y;
                double val = listRawdata[i].X * k + b;
                double dist = Math.Abs(y - val);
                //距离在阈值范围内
                if (dist <= _threshodY)
                {
                    var values = listRawdata.Skip(i).Take(range).ToList();
                    var xList = values.Select(n => n.X).ToArray();
                    var yList = values.Select(n => n.Y).ToArray();
                    var param = Fit.Line(xList, yList);

                    if (nCounter == 0)
                    {
                        if (param.Item2 > 0)
                        {
                            posCheck = 1;
                        }
                        else
                        {
                            posCheck = -1;
                        }
                    }
                    //方向判断
                    if (param.Item2 * posCheck > 0)
                    {
                        double angle = AngleConverter.ConvertRadToDegree(param.Item2);
                        //角度判断
                        if (angle < 0)
                        {
                            angle = 180 - angle;
                        }
                        //判定是否在阈值范围内
                        if (angle > _threadNominalParameters.fullAngle - _threshodAngle || angle < _threadNominalParameters.fullAngle + _threshodAngle)
                        {
                            //取三个值，第一第三个值的差值为一个螺距所对应的index。
                            if (nCounter == 3)
                            {
                                break;
                            }
                            else
                            {
                                listExtremPoints.Add(listRawdata[i]);
                                posCheck = -posCheck;//反转方向，保证每个牙侧面只取一个值
                                nCounter++;
                            }
                        }
                    }
                }
            }
            #endregion
            return listExtremPoints;
        }

        /// <summary>
        /// 寻找极值点位置
        /// </summary>
        /// <param name="listRawdata">原始数据点集</param>
        /// <param name="k">斜率值</param>
        /// <param name="b">选择位置</param>
        /// <param name="range">选取范围</param>
        public List<int> FindExtremPointsIndex(List<RawDataPoint> listRawdata, double k, double b, int range = 4)
        {
            var listExtremPoints = FindPoints(listRawdata, k, b, range);
            int firstPtIndex = listRawdata.IndexOf(listExtremPoints[0]);
            int secondPtIndex = listRawdata.IndexOf(listExtremPoints[1]);
            int thirdPtIndex = listRawdata.IndexOf(listExtremPoints[2]);

            int firstPt = (int)(secondPtIndex + firstPtIndex) / 2;
            int interval = Math.Abs(thirdPtIndex - firstPtIndex) / 2;//半个螺距取点

            List<int> listIndex = new List<int>();
            for (int i = firstPt; i < listRawdata.Count; i += interval)
            {
                listIndex.Add(i);
            }
            return listIndex;
        }

        /// <summary>
        /// 寻找最接近点的位置
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="k">斜率值</param>
        /// <param name="b">截距值</param>
        /// <returns>最接近该值的位置集合</returns>
        public List<int> FindClosetPointsIndex(List<RawDataPoint> listRawdata, double k, double b, int range)
        {
            var listExtremPoints = FindPoints(listRawdata, k, b, range);
            int firstPtIndex = listRawdata.IndexOf(listExtremPoints[0]);
            int secondPtIndex = listRawdata.IndexOf(listExtremPoints[1]);
            int thirdPtIndex = listRawdata.IndexOf(listExtremPoints[2]);
            int interval = Math.Abs(thirdPtIndex - firstPtIndex);//整个螺距取点

            List<int> listIndex = new List<int>();
            List<int> listOddNumberIndex = new List<int>();
            List<int> listEvenNumberIndex = new List<int>();
            for (int i = firstPtIndex; i < listRawdata.Count; i += interval)
            {
                listOddNumberIndex.Add(i);
            }
            for (int i = secondPtIndex; i < listRawdata.Count; i += interval)
            {
                listEvenNumberIndex.Add(i);
            }

            int listOddNumberLength = listOddNumberIndex.Count;
            int listEvenNumberLength = listEvenNumberIndex.Count;
            for (int i = 0; i < Math.Max(listOddNumberLength, listEvenNumberLength); i++)
            {
                if (i < listOddNumberLength)
                {
                    listIndex.Add(listOddNumberIndex[i]);
                }

                if (i < listEvenNumberLength)
                {
                    listIndex.Add(listEvenNumberIndex[i]);
                }
            }
            return listIndex;
        }

        /// <summary>
        /// 计算中径值（默认采用迭代计算）
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="majorParams">大径拟合参数(作为初始值参与迭代)</param>
        /// <returns>中径拟合参数</returns>
        public Tuple<double, double> CalculatePitchDiameter(List<RawDataPoint> listRawdata, Tuple<double, double> majorParams, Position position)
        {
            var obj = ObjectiveFunction.Value(GetDiffToothAndGroove);
            //采用无梯度优化中NelderMead算法计算
            var solver = new NelderMeadSimplex(convergenceTolerance: 1e-8, maximumIterations: 10000);

            Vector<double> initialGuess = CreateVector.Dense<double>(new double[2] { 0.0, 0.0 });
            if (position == Position.Top && _threadGaugeType == ThreadGaugeType.CylinderPlug)
            {
                initialGuess = CreateVector.Dense<double>(new double[2] { majorParams.Item1 - _toothHeight / 2, majorParams.Item2 }); //初始值
            }
            else
            {
                initialGuess = CreateVector.Dense<double>(new double[2] { majorParams.Item1 + _toothHeight / 2, majorParams.Item2 }); //初始值
            }

            //先确定牙侧
            listFullAngleParameters = FindAngleParameters(listRawdata, initialGuess[1], initialGuess[0], out _listFitAnglePoints);
            var result = solver.FindMinimum(obj, initialGuess);

            //再次根据迭代值计算
            listFullAngleParameters = FindAngleParameters(listRawdata, result.MinimizingPoint[1], result.MinimizingPoint[0], out _listFitAnglePoints);
            result = solver.FindMinimum(obj, CreateVector.Dense<double>(new double[2] { result.MinimizingPoint[0], result.MinimizingPoint[1] }));
            //Console.WriteLine("Point:\t" + "NelderMead:" + result.MinimizingPoint[1] + " , " + result.MinimizingPoint[0] + " , "+result.Iterations);
            return new Tuple<double, double>(result.MinimizingPoint[0], result.MinimizingPoint[1]);
        }

        /// <summary>
        /// 计算均值
        /// </summary>
        /// <param name="listdata">数据集</param>
        protected double GetAverage(System.Collections.Generic.List<ThreadModule.Data.RawDataPoint> listdata)
        {
            return listdata.Select(n => n.Y).Average();
        }

        /// <summary>
        /// 计算重心（斜率值k对中径等影响较大，最终依据y=kx+b计算，x取均值）
        /// </summary>
        /// <param name="listdata">数据点集</param>
        private static double GetCenterOfGravity(System.Collections.Generic.List<ThreadModule.Data.RawDataPoint> listdata)
        {
            return listdata.Select(n => n.X).Average();
        }

        /// <summary>
        /// 计算总齿宽和齿槽宽差值
        /// </summary>
        /// <param name="inputParams">两个参数,分别是斜率k,中径截距b</param>
        /// <returns>总齿宽和齿槽宽差值</returns>
        private double GetDiffToothAndGroove(Vector<double> inputParams)
        {
            double sumDist = 0.0;
            double k = inputParams[1];
            double b = inputParams[0];

            #region 核心算法
            //计算相邻牙距离差，累加求最小值
            for (int i = 0; i < listFullAngleParameters.Count - 2; i += 2)
            {
                double k1 = listFullAngleParameters[i].Item2;
                double k2 = listFullAngleParameters[i + 1].Item2;
                double k3 = listFullAngleParameters[i + 2].Item2;
                double b1 = listFullAngleParameters[i].Item1;
                double b2 = listFullAngleParameters[i + 1].Item1;
                double b3 = listFullAngleParameters[i + 2].Item1;
                RawDataPoint pt1 = GetIntersectPoint(k, b, k1, b1);
                RawDataPoint pt2 = GetIntersectPoint(k, b, k2, b2);
                RawDataPoint pt3 = GetIntersectPoint(k, b, k3, b3);
                double distTooth = Math.Sqrt(Math.Pow(pt1.X - pt2.X, 2) + Math.Pow(pt1.Y - pt2.Y, 2));
                double distGroove = Math.Sqrt(Math.Pow(pt3.X - pt2.X, 2) + Math.Pow(pt3.Y - pt2.Y, 2));
                sumDist += Math.Abs(distTooth - distGroove);
            }
            #endregion
            return sumDist;
        }

        /// <summary>
        /// 计算牙侧角参数值
        /// </summary>
        /// <param name="listRawdata">数据点集</param>
        /// <param name="k">斜率值</param>
        /// <param name="b">截距值</param>
        /// <param name="listFitAnglePoints">各牙侧角拟合数据点集合</param>
        /// <param name="range">取值范围</param>
        /// <returns>牙侧角参数值</returns>
        public List<Tuple<double, double>> FindAngleParameters(List<RawDataPoint> listRawdata, double k, double b,out List<List<RawDataPoint>> listFitAnglePoints, int range = 20)
        {
            List<int> listIndex = FindClosetPointsIndex(listRawdata, k, b, range);
            listFitAnglePoints = new List<List<RawDataPoint>>();
            //拟合牙侧线
            _listFullAngleParameters = new List<Tuple<double, double>>();
            for (int i = 0; i < listIndex.Count; i++)
            {
                int index = listIndex[i];
                //考虑数组界限问题
                List<RawDataPoint> points = new List<RawDataPoint>();
                if (index < listRawdata.Count - range / 2)
                {
                    points = listRawdata.Skip(index - range / 2).Take(range).ToList();
                }
                else if (index >= listRawdata.Count)
                {
                    points = listRawdata.Take(index - range).ToList();
                }
                else if (index < range / 2)
                {
                    points = listRawdata.Take(index + range).ToList();
                }
                var value = Fit.Line(points.Select(n => n.X).ToArray(), points.Select(n => n.Y).ToArray());
                var temp = points.Select(n => new RawDataPoint() { X = n.X, Y = n.X * value.Item2 + value.Item1 }).ToList();//计算拟合的牙侧角数据点
                _listFullAngleParameters.Add(value);
                listFitAnglePoints.Add(temp);//牙侧角数据点添加到集合中
            }

            int nToothCount = _listFullAngleParameters.Count;//总牙数
            bool isOddNumber = nToothCount % 2 == 1 ? true : false;
            List<Tuple<double, double>> listFullAngleParameters = _listFullAngleParameters;
            if (!isOddNumber)
            {
                listFullAngleParameters.RemoveAt(listFullAngleParameters.Count - 1);
            }
            return listFullAngleParameters;
        }

        /// <summary>
        /// 计算两直线交点
        /// </summary>
        /// <param name="k1">斜率值1</param>
        /// <param name="b1">截距值1</param>
        /// <param name="k2">斜率值2</param>
        /// <param name="b2">截距值2</param>
        /// <returns>交点值</returns>
        protected RawDataPoint GetIntersectPoint(double k1, double b1, double k2, double b2)
        {
            double x0 = (b2 - b1) / (k1 - k2);
            double y0 = (k1 * b2 - k2 * b1) / (k1 - k2);
            return new RawDataPoint() { X = x0, Y = y0 };
        }

        /// <summary>
        /// 计算两条直线之间的夹角
        /// </summary>
        /// <param name="k1">斜率1</param>
        /// <param name="k2">斜率2</param>
        /// <returns>夹角角度值</returns>
        protected double GetAngleBetweenLines(double k1, double k2)
        {
            if (k1 * k2 == -1)
            {
                return 90.0;
            }
            else
            {
                double angle = AngleConverter.ConvertRadToDegree(Math.Atan(Math.Abs((k1 - k2) / (1 + k1 * k2))));
                return angle;
            }
        }

        [Conditional("DEBUG")]
        protected void ShowOutputParameters(ThreadParameters measuredThreadParameter)
        {
            measuredThreadParameter.Dump();
        }

        [Conditional("DEBUG")]
        protected void ShowCalculatedParameters(CalculatedThreadParameter calculatedThreadParameter)
        {
            calculatedThreadParameter.Dump();
        }
    }
}
